#include "tio_sensor.h"
#include "log/zlog.hh"

static uint8_t MyCRC_GetCRC8(uint8_t* pDat, uint8_t bytes)
{
    uint8_t CRC8_POLY = 0x8C;  // X^8 + X^5 + X^4 + 1
    uint8_t crc;
    uint8_t temp = 0x00, nLen = 0x00;
    uint8_t i = 0x00;
    crc = 0x00;
    nLen = bytes;
    //crc check
    while (nLen--)
    {
        crc = crc ^ *pDat;
        for (i = 0x00; i < 8; i++)
        {
            temp = (crc & 0x01);
            crc >>= 1;
            if (temp)
                crc ^= CRC8_POLY;
        }
        pDat++;
    }
    return crc;
}

rtdev::TIOSensor::TIOSensor() {}

rtdev::TIOSensor::~TIOSensor() {}

void rtdev::TIOSensor::doFilt(const double* raw_data)
{
    if (lowpass_filter_frq_ > 0.0)
    {
        double cutFrq, RC, cof1, cof2;
        cutFrq = lowpass_filter_frq_;
        RC = 1.0 / cutFrq / 2.0 / 3.1415926;
        cof1 = 1 / (1 + RC / 0.008);
        cof2 = RC / 0.008 / (1 + RC / 0.008);
        for (int i = 0; i < 6; i++)
        {
            sensor_data_[i] = cof1 * raw_data[i] + cof2 * last_sensor_data_[i];
            //update
            last_sensor_data_[i] = sensor_data_[i];
        }
    }
    else
    {
        for (int i = 0; i < 6; i++) { sensor_data_[i] = raw_data[i]; }
    }
}

// 传感器数据格式   AA 55 XX XX XX XX XX XX XX XX XX CRC, 共12位数据
int rtdev::TIOSensor::decodeSensor(uint8_t* buf, int len)
{
    if (buf == NULL)
        return -1;
    if (len != SNESOR_DATA_LENGTH)  // 丢弃错误格式的报文
    {
        return -1;
    }

    if (MyCRC_GetCRC8(buf + 2, 9) != buf[SNESOR_DATA_LENGTH - 1])  // 中间9个数据字节进行校验
    {
        return -1;
    }

    uint32_t tio_torque[6] = {};
    tio_torque[0] = (buf[2] << 6) + ((buf[3] & 0xFC) >> 2);
    tio_torque[1] = ((buf[3] & 0x03) << 12) + ((buf[4] & 0xFF) << 4) + ((buf[5] & 0xF0) >> 4);
    tio_torque[2] = ((buf[5] & 0x0F) << 10) + ((buf[6] & 0xFF) << 2) + ((buf[7] & 0xC0) >> 6);
    tio_torque[3] = ((buf[7] & 0x3F) << 4) + ((buf[8] & 0xF0) >> 4);
    tio_torque[4] = ((buf[8] & 0x0F) << 6) + ((buf[9] & 0xFC) >> 2);
    tio_torque[5] = ((buf[9] & 0x03) << 8) + buf[10];

    for (int i = 0; i < 6; i++)
    {
        if (i < 3 && (tio_torque[i] & 0x2000))
        {
            sensor_raw_data_[i] = -1 * (tio_torque[i] & 0x1FFF);
        }
        else if (i >= 3 && (tio_torque[i] & 0x200))
        {
            tio_torque[i] = -1 * (tio_torque[i] & 0x1FF);
        }
        sensor_raw_data_[i] = tio_torque[i] / 10.0;
    }
    doFilt(sensor_raw_data_);
    return 0;
}

void rtdev::TIOSensor::comm_read_sensor_data(uint8_t* tio_rs485_buf, int len)
{
    keep_alive();
    if (0 != decodeSensor(tio_rs485_buf, len))
    {
        data_err_count_--;
    }
    else
    {
        data_err_count_ = TIO_SENSOR_WATCDOG_COUNT;
    }
}

void rtdev::TIOSensor::update()
{
    check_alive();
    if (data_err_count_ < 0)
    {
        data_err_count_ = 0;
        tio_sensor_err_ = TIO_SENSOR_REC_ERR;
    }
}
void rtdev::TIOSensor::keep_alive() {}
void rtdev::TIOSensor::check_alive()
{
    bool alive = false;
    if (sim_flag_)
    {
        alive = true;
    }
    else
    {
        if (alive_watchdog_ <= 0)
        {
            alive = false;
        }
        else
        {
            alive_watchdog_--;
            alive = true;
        }
    }
    if (alive != alive_flag_)
    {
        if (alive)
        {
            zlog()->info("TioSensor is online, sim=%d", sim_flag_);
        }
        else
        {
            zlog()->info("TioSensor is offline, sim=%d", sim_flag_);
        }
        alive_flag_ = alive;
    }
}
